# pragma once

// Qt support
# include <QApplication>
# include <QMainWindow>
# include <QDebug>
# include <QButtonGroup>

# include "cpp_tools.h"
# include "qcustomplot.h"
# include "udpSettings.h"

class scope : public QMainWindow
{
    protected:
    QLabel *label1; // info
    QLabel *label2; // 显示延迟
    QCustomPlot *customPlot;    // QCustomPlot 控件
    QPushButton *button1;       // 频道-
    QPushButton *button2;       // 频道+

    QCPItemText *text1;

    QTimer *timer;              // 定时器

    int m_chanel = 1; // 当前播放频道[1,inf)，其他数值无效
    const int tolChanel = 9; // 总频道数: 2D map, v, beta, omega, a_x, a_y, delta, drive, comput_time
    
    size_t n_data = 0;
    const size_t n_data_max = 1e3;
    cpp_tools::mtDataManager<Eigen::MatrixXd> plotData;
    // 数据, t, x, y, varphi, v, beta, omega, a_x, a_y, delta, F_x, comput_time 共12个

    const size_t n_pos_max = 300;
    size_t n_pos = 0;
    cpp_tools::mtDataManager<Eigen::MatrixXd> posData; // 记录过去一段时间的x,y

    bool isRoadOK = false;
    Eigen::VectorXd road_center_x;
    Eigen::VectorXd road_center_y;
    Eigen::VectorXd road_left_x;
    Eigen::VectorXd road_left_y;
    Eigen::VectorXd road_right_x;
    Eigen::VectorXd road_right_y;
    Eigen::VectorXd road_box;

    QCPCurve * roadCenter;
    QCPCurve * roadLeft;
    QCPCurve * roadRight;

    QCPCurve * vehCenter;
    QCPCurve * vehBody;
    QCPCurve * vehFront;
    QCPCurve * vehRear;
    QCPCurve * vehWheel1;
    QCPCurve * vehWheel2;
    QCPCurve * vehWheel3;
    QCPCurve * vehWheel4;
    QCPCurve * vehTrack;

    double l_f = 0.215;
    double l_r = 0.216;
    double l_y = 0.295;
    double R_wheel = 0.048;

    /// @brief 画道路
    void dispRoad()
    {
        if (isRoadOK)
        {
            int len = road_center_x.size();

            QVector<double> QxData(len);
            QVector<double> QyData(len);
            
            {
                std::memcpy(QxData.data(),road_center_x.data(),len*sizeof(double));
                std::memcpy(QyData.data(),road_center_y.data(),len*sizeof(double));

                roadCenter->data()->clear();
                roadCenter->setData(QxData, QyData);

                QPen pen;
                pen.setColor(QColor(155,155,155));
                pen.setWidth(2);
                pen.setStyle(Qt::DashLine);
                roadCenter->setPen(pen);
            }

            {
                std::memcpy(QxData.data(),road_left_x.data(),len*sizeof(double));
                std::memcpy(QyData.data(),road_left_y.data(),len*sizeof(double));

                roadLeft->data()->clear();
                roadLeft->setData(QxData, QyData);

                QPen pen;
                pen.setColor(QColor(0,0,0));
                pen.setWidth(2);
                pen.setStyle(Qt::SolidLine);
                roadLeft->setPen(pen);
            }

            {
                std::memcpy(QxData.data(),road_right_x.data(),len*sizeof(double));
                std::memcpy(QyData.data(),road_right_y.data(),len*sizeof(double));

                roadRight->data()->clear();
                roadRight->setData(QxData, QyData);

                QPen pen;
                pen.setColor(QColor(0,0,0));
                pen.setWidth(2);
                pen.setStyle(Qt::SolidLine);
                roadRight->setPen(pen);
            }

            customPlot->replot();
        }
    };

    /// @brief 画车
    void dispVehicle()
    {
        if (n_data == 0)
        {
            return;
        }
        
        double x = (*plotData.read().access())(n_data - 1,1); 
        double y = (*plotData.read().access())(n_data - 1,2); 
        double varphi = (*plotData.read().access())(n_data - 1,3); 
        double delta = (*plotData.read().access())(n_data - 1,9);

        double v = (*plotData.read().access())(n_data - 1,4);
        double text_x = x;
        double text_y = y - 0.2
        - std::abs(std::max(l_f,l_r)*std::sin(varphi)) 
        - std::abs(R_wheel*std::sin(varphi)) 
        - std::abs(0.5*l_y*std::cos(varphi));
        text1->position->setCoords(text_x,text_y);
        text1->setText(QString::number(v,'f',2) + "m/s");

        double x_min = x;
        double y_min = text_y - 0.2;
        double x_max = x;
        double y_max = y;

        // 左前
        double pose_x1 = x + l_f*std::cos(varphi) - l_y/2*std::sin(varphi);
        double pose_y1 = y + l_f*std::sin(varphi) + l_y/2*std::cos(varphi);

        x_min = std::min(x_min,pose_x1);
        x_max = std::max(x_max,pose_x1);
        y_min = std::min(y_min,pose_y1);
        y_max = std::max(y_max,pose_y1);

        // 右前
        double pose_x2 = x + l_f*std::cos(varphi) + l_y/2*std::sin(varphi);
        double pose_y2 = y + l_f*std::sin(varphi) - l_y/2*std::cos(varphi);

        x_min = std::min(x_min,pose_x2);
        x_max = std::max(x_max,pose_x2);
        y_min = std::min(y_min,pose_y2);
        y_max = std::max(y_max,pose_y2);

        // 左后
        double pose_x3 = x - l_r*std::cos(varphi) - l_y/2*std::sin(varphi);
        double pose_y3 = y - l_r*std::sin(varphi) + l_y/2*std::cos(varphi);

        x_min = std::min(x_min,pose_x3);
        x_max = std::max(x_max,pose_x3);
        y_min = std::min(y_min,pose_y3);
        y_max = std::max(y_max,pose_y3);

        // 右后
        double pose_x4 = x - l_r*std::cos(varphi) + l_y/2*std::sin(varphi);
        double pose_y4 = y - l_r*std::sin(varphi) - l_y/2*std::cos(varphi);

        x_min = std::min(x_min,pose_x4);
        x_max = std::max(x_max,pose_x4);
        y_min = std::min(y_min,pose_y4);
        y_max = std::max(y_max,pose_y4);

        // 左前
        double wheel_center_x1 = pose_x1;
        double wheel_center_y1 = pose_y1;

        double wheel_front_x1 = wheel_center_x1 + R_wheel*cos(varphi + delta);
        double wheel_front_y1 = wheel_center_y1 + R_wheel*sin(varphi + delta);

        x_min = std::min(x_min,wheel_front_x1);
        x_max = std::max(x_max,wheel_front_x1);
        y_min = std::min(y_min,wheel_front_y1);
        y_max = std::max(y_max,wheel_front_y1);

        double wheel_rear_x1 = wheel_center_x1 - R_wheel*cos(varphi + delta);
        double wheel_rear_y1 = wheel_center_y1 - R_wheel*sin(varphi + delta);

        x_min = std::min(x_min,wheel_rear_x1);
        x_max = std::max(x_max,wheel_rear_x1);
        y_min = std::min(y_min,wheel_rear_y1);
        y_max = std::max(y_max,wheel_rear_y1);

        // 右前
        double wheel_center_x2 = pose_x2;
        double wheel_center_y2 = pose_y2;

        double wheel_front_x2 = wheel_center_x2 + R_wheel*cos(varphi + delta);
        double wheel_front_y2 = wheel_center_y2 + R_wheel*sin(varphi + delta);

        x_min = std::min(x_min,wheel_front_x2);
        x_max = std::max(x_max,wheel_front_x2);
        y_min = std::min(y_min,wheel_front_y2);
        y_max = std::max(y_max,wheel_front_y2);

        double wheel_rear_x2 = wheel_center_x2 - R_wheel*cos(varphi + delta);
        double wheel_rear_y2 = wheel_center_y2 - R_wheel*sin(varphi + delta);

        x_min = std::min(x_min,wheel_rear_x2);
        x_max = std::max(x_max,wheel_rear_x2);
        y_min = std::min(y_min,wheel_rear_y2);
        y_max = std::max(y_max,wheel_rear_y2);

        // 左后
        double wheel_center_x3 = pose_x3;
        double wheel_center_y3 = pose_y3;

        double wheel_front_x3 = wheel_center_x3 + R_wheel*cos(varphi);
        double wheel_front_y3 = wheel_center_y3 + R_wheel*sin(varphi);

        x_min = std::min(x_min,wheel_front_x3);
        x_max = std::max(x_max,wheel_front_x3);
        y_min = std::min(y_min,wheel_front_y3);
        y_max = std::max(y_max,wheel_front_y3);

        double wheel_rear_x3 = wheel_center_x3 - R_wheel*cos(varphi);
        double wheel_rear_y3 = wheel_center_y3 - R_wheel*sin(varphi);

        x_min = std::min(x_min,wheel_rear_x3);
        x_max = std::max(x_max,wheel_rear_x3);
        y_min = std::min(y_min,wheel_rear_y3);
        y_max = std::max(y_max,wheel_rear_y3);

        // 右后
        double wheel_center_x4 = pose_x4;
        double wheel_center_y4 = pose_y4;

        double wheel_front_x4 = wheel_center_x4 + R_wheel*cos(varphi);
        double wheel_front_y4 = wheel_center_y4 + R_wheel*sin(varphi);

        x_min = std::min(x_min,wheel_front_x4);
        x_max = std::max(x_max,wheel_front_x4);
        y_min = std::min(y_min,wheel_front_y4);
        y_max = std::max(y_max,wheel_front_y4);

        double wheel_rear_x4 = wheel_center_x4 - R_wheel*cos(varphi);
        double wheel_rear_y4 = wheel_center_y4 - R_wheel*sin(varphi);

        x_min = std::min(x_min,wheel_rear_x4);
        x_max = std::max(x_max,wheel_rear_x4);
        y_min = std::min(y_min,wheel_rear_y4);
        y_max = std::max(y_max,wheel_rear_y4);

        // 开始画图
        vehCenter->data()->clear();
        vehBody->data()->clear();
        vehFront->data()->clear();
        vehRear->data()->clear();
        vehWheel1->data()->clear();
        vehWheel2->data()->clear();
        vehWheel3->data()->clear();
        vehWheel4->data()->clear();
        vehTrack->data()->clear();

        {
            vehFront->addData(pose_x1,pose_y1);
            vehFront->addData(pose_x2,pose_y2);

            QPen pen;
            pen.setColor(QColor(225,0,25));
            pen.setWidth(3);
            pen.setStyle(Qt::SolidLine);
            vehFront->setPen(pen);
        }

        {
            vehRear->addData(pose_x3,pose_y3);
            vehRear->addData(pose_x4,pose_y4);

            QPen pen;
            pen.setColor(QColor(173,158,85));
            pen.setWidth(3);
            pen.setStyle(Qt::SolidLine);
            vehRear->setPen(pen);
        }

        {
            vehBody->addData((pose_x1 + pose_x2)/2.,(pose_y1 + pose_y2)/2.);
            vehBody->addData((pose_x3 + pose_x4)/2.,(pose_y3 + pose_y4)/2.);

            QPen pen;
            pen.setColor(QColor(173,158,85));
            pen.setWidth(3);
            pen.setStyle(Qt::SolidLine);
            vehBody->setPen(pen);
        }

        {
            vehCenter->addData(x,y);

            QPen pen;
            pen.setColor(QColor(255,0,0));
            pen.setWidth(2);
            pen.setStyle(Qt::SolidLine);
            vehCenter->setPen(pen);

            vehCenter->setBrush(QBrush(QColor(255,0,0)));
            vehCenter->setScatterStyle(QCPScatterStyle(QCPScatterStyle::ssCircle,10));
        }

        {
            vehWheel1->addData(wheel_front_x1,wheel_front_y1);
            vehWheel1->addData(wheel_rear_x1,wheel_rear_y1);

            QPen pen;
            pen.setColor(QColor(54,40,43));
            pen.setWidth(4);
            pen.setStyle(Qt::SolidLine);
            vehWheel1->setPen(pen);
        }

        {
            vehWheel2->addData(wheel_front_x2,wheel_front_y2);
            vehWheel2->addData(wheel_rear_x2,wheel_rear_y2);

            QPen pen;
            pen.setColor(QColor(54,40,43));
            pen.setWidth(4);
            pen.setStyle(Qt::SolidLine);
            vehWheel2->setPen(pen);
        }
        
        {
            vehWheel3->addData(wheel_front_x3,wheel_front_y3);
            vehWheel3->addData(wheel_rear_x3,wheel_rear_y3);

            QPen pen;
            pen.setColor(QColor(54,40,43));
            pen.setWidth(4);
            pen.setStyle(Qt::SolidLine);
            vehWheel3->setPen(pen);
        }

        {
            vehWheel4->addData(wheel_front_x4,wheel_front_y4);
            vehWheel4->addData(wheel_rear_x4,wheel_rear_y4);

            QPen pen;
            pen.setColor(QColor(54,40,43));
            pen.setWidth(4);
            pen.setStyle(Qt::SolidLine);
            vehWheel4->setPen(pen);
        }

        {
            QVector<double> QxData(n_pos);
            QVector<double> QyData(n_pos);

            Eigen::VectorXd posX = posData.read().access()->leftCols(1).topRows(n_pos);
            Eigen::VectorXd posY = posData.read().access()->rightCols(1).topRows(n_pos);

            std::memcpy(QxData.data(),posX.data(),n_pos*sizeof(double));
            std::memcpy(QyData.data(),posY.data(),n_pos*sizeof(double));

            vehTrack->addData(QxData,QyData);

            QPen pen;
            pen.setColor(QColor(0,0,225));
            pen.setWidth(1);
            pen.setStyle(Qt::SolidLine);
            vehTrack->setPen(pen);
            vehTrack->setScatterStyle(QCPScatterStyle::ssPlus);
        }

        double h = customPlot->geometry().height();
        double l = customPlot->geometry().width();

        double x_center = (road_box(0) + road_box(2))/2;
        double y_center = (road_box(1) + road_box(3))/2; 

        double x_dis = std::max(std::max(std::abs(x_center - x_max),std::abs(x_center - x_min)),std::abs(road_box(0) - road_box(2))/2);
        double y_dis = std::max(std::max(std::abs(y_center - y_max),std::abs(y_center - y_min)),std::abs(road_box(1) - road_box(3))/2);
        if (x_dis/l >= y_dis/h)
        {
            y_dis = x_dis/l*h;
        }
        else
        {
            x_dis = y_dis/h*l;
        }

        customPlot->xAxis->setRange(x_center - x_dis,x_center + x_dis);
        customPlot->yAxis->setRange(y_center - y_dis,y_center + y_dis);

        customPlot->replot();
    };

    /// @brief 设置风格
    void setupChanelStyle()
    {
        QPen pen;

        int ChanelNumber = 1;
        if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("GLOB X (m)");
            customPlot->yAxis->setLabel("GLOB Y (m)");
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("v (m/s)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("beta (rad)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("omega (rad/s)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("a_x (m/s^2)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("a_y (m/s^2)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("delta (-)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("drive (-)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(1);
            customPlot->graph(0)->setPen(pen);
        }
        else if (m_chanel == ChanelNumber++)
        {
            customPlot->xAxis->setLabel("time (s)");
            customPlot->yAxis->setLabel("comput time (ms)");

            pen.setColor(QColor(255,0,0));
            pen.setWidth(0.5);
            customPlot->graph(0)->setPen(pen);
        }
    };

    /// @brief 频道切换
    void chanelSwitch()
    {
        while (m_chanel > tolChanel || m_chanel <= 0)
        {
            if (m_chanel > tolChanel)
            {
                m_chanel -= tolChanel;
            }
            else if (m_chanel <= 0)
            {
                m_chanel += tolChanel;
            }
        }

        setupChanelStyle();

        customPlot->graph(0)->data()->clear();

        customPlot->replot();

        if (m_chanel == 1)
        {
            text1->setVisible(true);

            roadCenter->setVisible(true);
            roadLeft->setVisible(true);
            roadRight->setVisible(true);

            dispVehicle();

            customPlot->xAxis->rescale(true);
            customPlot->yAxis->rescale(true);
        }
        else
        {
            text1->setVisible(false);

            roadCenter->setVisible(false);
            roadLeft->setVisible(false);
            roadRight->setVisible(false);

            vehCenter->data()->clear();
            vehBody->data()->clear();
            vehFront->data()->clear();
            vehRear->data()->clear();
            vehWheel1->data()->clear();
            vehWheel2->data()->clear();
            vehWheel3->data()->clear();
            vehWheel4->data()->clear();
            vehTrack->data()->clear();

            Eigen::VectorXd xData;
            Eigen::VectorXd yData;

            double key;

            if (n_data == 0)
            {
                return;
            }

            auto ac = plotData.read();

            xData = (*ac.access())(Eigen::seqN(0,n_data), 0);
            yData = (*ac.access())(Eigen::seqN(0,n_data), m_chanel + 2);

            key = (*ac.access())(n_data - 1, 0);

            QVector<double> QxData(xData.size());
            QVector<double> QyData(yData.size());

            std::memcpy(QxData.data(),xData.data(),xData.size()*sizeof(double));
            std::memcpy(QyData.data(),yData.data(),yData.size()*sizeof(double));

            customPlot->graph(0)->setData(QxData,QyData);

            customPlot->xAxis->setRange(key, 8, Qt::AlignRight);
            customPlot->yAxis->rescale(true);
        }

        customPlot->replot();
    }

    /// @brief 刷新
    void refreshPlot()
    {
        if (m_chanel == 1)
        {
            dispVehicle();
        }
        else
        {
            if (n_data == 0)
            {
                return;
            }

            auto ac = plotData.read();
            customPlot->xAxis->setRange((*ac.access())(n_data - 1,0), 8, Qt::AlignRight);
            customPlot->yAxis->rescale(true);
        }

        customPlot->replot();
    };

public:
    explicit scope(QWidget *parent = nullptr)
        : QMainWindow(parent)
    {
        setWindowTitle("scope");
        setGeometry(500,200,1200,800);

        // 初始化数据
        plotData.write().access()->resize(n_data_max,12);
        posData.write().access()->resize(n_pos_max,2);

        // 读取路径文件
        std::string settingsFile = host_path + settings_path + "/disp/settings.xml";
        cpp_tools::XmlParameterManager m_xml("settings", settingsFile);
        std::string road_name = "";
        bool flag = m_xml.getParameter("", "road", road_name);

        if (!flag || road_name.empty())
        {
            m_xml.setParameter("", "road", road_name);
            m_xml.saveToFile(settingsFile);
            throw std::runtime_error("无效的设置， 请重新编辑文件：" + settingsFile);
        }

        auto trackData = cpp_tools::csvRead(host_path + road_path + road_name);
        if (trackData.size() != 0)
        {
            std::cout << "成功获取2D道路" << std::endl;

            int i = 0;
            road_center_x = trackData(Eigen::all,i++);
            road_center_y = trackData(Eigen::all,i++);
            road_left_x = trackData(Eigen::all,i++);
            road_left_y = trackData(Eigen::all,i++);
            road_right_x = trackData(Eigen::all,i++);
            road_right_y = trackData(Eigen::all,i++);

            road_box.resize(4);
            road_box << 
                std::min(road_left_x.minCoeff(),road_right_x.minCoeff()) - 1,
                std::min(road_left_y.minCoeff(),road_right_y.minCoeff()) - 1,
                std::max(road_left_x.maxCoeff(),road_right_x.maxCoeff()) + 1,
                std::max(road_left_y.maxCoeff(),road_right_y.maxCoeff()) + 1;

            isRoadOK = true;
        }
        else
        {
            std::cout << "加载2D道路失败, 找不到文件: " << host_path + road_path + road_name << std::endl;
        }

        // 创建 QCustomPlot 控件
        customPlot = new QCustomPlot(this);

        // 设置 QCustomPlot 的基本属性
        customPlot->addGraph(); // 添加一个图形
        roadCenter = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        roadLeft = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        roadRight = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehCenter = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehBody = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehFront = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehRear = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehWheel1 = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehWheel2 = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehWheel3 = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehWheel4 = new QCPCurve(customPlot->xAxis, customPlot->yAxis);
        vehTrack = new QCPCurve(customPlot->xAxis, customPlot->yAxis);

        text1 = new QCPItemText(customPlot);
        text1->setText(" m/s");
        text1->position->setType(QCPItemPosition::ptPlotCoords);
        text1->position->setCoords(0,0);
        text1->setFont(QFont(font().family(),13));

        dispRoad();

        chanelSwitch();

        // 创建label
        label1 = new QLabel(this);
        label1->setFixedSize(0.85*this->geometry().width(), 30);
        label1->setWordWrap(true);
        label1->setAlignment(Qt::AlignVCenter);
        label1->setStyleSheet("background-color: white; border: 3px solid black; color: black; font-size: 16px;");
        label1->setText("INFO: ");

        label2 = new QLabel(this);
        label2->setFixedSize(0.12*this->geometry().width(),30);
        label2->setAlignment(Qt::AlignCenter);
        label2->setStyleSheet("background-color: white; border: 3px solid black; color: black; font-size: 16px;");
        label2->setText("ping: ms");

        // 创建按钮
        button1 = new QPushButton("频道 -", this);
        button2 = new QPushButton("频道 +", this);

        // 创建布局
        QGridLayout *layout = new QGridLayout;
        layout->addWidget(label1,0,0,1,2);
        layout->addWidget(label2,0,2);
        layout->addWidget(customPlot,1,0,1,3);
        layout->addWidget(button1,2,0);
        layout->addWidget(button2,2,1);

        // 设置窗口的中心部件
        QWidget *centralWidget = new QWidget(this);
        centralWidget->setLayout(layout);
        setCentralWidget(centralWidget);

        // 连接按钮信号与槽
        connect(button1, &QPushButton::clicked, this, [&]() -> void{
            m_chanel -= 1;
            chanelSwitch();
        });
        connect(button2, &QPushButton::clicked, this, [&]() -> void{
            m_chanel += 1;
            chanelSwitch();
        });

        // make left and bottom axes transfer their ranges to right and top axes:
        customPlot->axisRect()->setupFullAxesBox();
        connect(customPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), customPlot->xAxis2, SLOT(setRange(QCPRange)));
        connect(customPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), customPlot->yAxis2, SLOT(setRange(QCPRange)));

        // 设置定时器
        timer = new QTimer(this);
        connect(timer, &QTimer::timeout, this, [&]() -> void{
            refreshPlot();
        });
        timer->start(10); // 每10ms更新一次数据
    };

    ~scope()
    {

    };

    /// @brief 添加新数据 
    void addPlotData(const Eigen::VectorXd & newPlotData)
    {
        {
            auto ac = plotData.write();

            if (n_data == n_data_max)
            {
                ac.access()->topRows(n_data_max - 1) = ac.access()->bottomRows(n_data_max - 1);
                
                ac.access()->bottomRows(1) = newPlotData.transpose();
            }
            else
            {
                ac.access()->row(n_data) = newPlotData.transpose();

                n_data += 1;
            }

            if (m_chanel == 1)
            {
                /* code */
            }
            else if (m_chanel > 1 && m_chanel <= 9)
            {
                customPlot->graph(0)->addData(newPlotData(0),newPlotData(m_chanel + 2));
            }
        }

        {
            auto ac = posData.write();
            Eigen::VectorXd newPos(2);
            newPos << newPlotData(1),newPlotData(2);

            if (n_pos == n_pos_max)
            {
                ac.access()->topRows(n_pos_max - 1) = ac.access()->bottomRows(n_pos_max - 1);

                ac.access()->bottomRows(1) = newPos.transpose();
            }
            else
            {
                ac.access()->row(n_pos) = newPos.transpose();

                n_pos += 1;
            }
        }
    };

    void dispInfo(QString info)
    {
        QString str;
        str = "INFO: " + info;
        label1->setText(str);
    }

    void dispDelay(double t)
    {
        QString str;
        str = "ping: " + QString::number(t) + " ms";
        label2->setText(str);
    }
};
